﻿namespace ElBruno.MerryChristmas
{
    using System.Windows;

    using Microsoft.Research.Kinect.Nui;

    using System;
    using System.Diagnostics;
    using System.Linq;
    using System.Windows.Controls;
    using System.Windows.Media;
    using System.Windows.Shapes;

    /// <summary>
    /// Interaction logic for MainWindow.xaml
    /// </summary>
    public partial class MainWindow
    {
        private Runtime kinect;
        private InteropBitmapHelper imageHelper;

        private KinectCanvas kinectCanvas;

        public const RuntimeOptions KinectRuntimeOptions =
            RuntimeOptions.UseDepthAndPlayerIndex |
            RuntimeOptions.UseSkeletalTracking |
            RuntimeOptions.UseColor;

        public MainWindow()
        {
            InitializeComponent();
            Loaded += MainWindowLoaded;
        }

        void MainWindowLoaded(object sender, RoutedEventArgs e)
        {
            this.FindKinectSensor();
        }

        #region Kinect Skeleton Management
        void KinectSkeletonFrameReady(object sender, SkeletonFrameReadyEventArgs e)
        {
            canvasSkeleton.Children.Clear();
            this.HideImageHats();
            var skeletonId = 0;
            foreach (var skeleton in e.SkeletonFrame.Skeletons.Where
                          (skeleton =>
                                    SkeletonTrackingState.Tracked == skeleton.TrackingState))
            {
                skeletonId++;
                this.DisplayImageHats(skeletonId);
                PaintBones(skeleton);
                PaintJoints(skeleton);

                foreach (Joint joint in skeleton.Joints)
                {
                    if (joint.ID != JointID.Head)
                    {
                        continue;
                    }


                    float depthX;
                    float depthY;
                    kinect.SkeletonEngine.SkeletonToDepthImage(joint.Position, out depthX, out depthY);
                    var depthPixelX = (int)Math.Max(0, Math.Min(depthX * CameraViewer.ActualWidth, CameraViewer.ActualWidth - 1));
                    var depthPixelY = (int)Math.Max(0, Math.Min(depthY * CameraViewer.ActualHeight, CameraViewer.ActualHeight - 1));

                    depthPixelY -= 20;

                    var newPosition = this.GetDisplayPosition(joint);
                    this.MoveImageHats(newPosition, skeletonId);

                    Debug.WriteLine(string.Format("{0} ", DateTime.Now.TimeOfDay));
                    Debug.WriteLine(string.Format("    X:{0} Y:{1}", joint.Position.X, joint.Position.Y));
                    Debug.WriteLine(string.Format("    W:{0} Z:{1}", joint.Position.W, joint.Position.Z));
                    Debug.WriteLine(string.Format("    SkeletonToDepthImage depthX:{0} depthY:{1}", depthX, depthY));
                    Debug.WriteLine(string.Format("    depthPixelX:{0} depthPixelY:{1}", depthPixelX, depthPixelY));
                    Debug.WriteLine(string.Format("    New position X:{0} Y:{1}", newPosition.X, newPosition.Y));
                }
            }
        }

        private Point GetDisplayPosition(Joint joint)
        {
            float depthX, depthY;
            kinect.SkeletonEngine.SkeletonToDepthImage(joint.Position, out depthX, out depthY);
            depthX = Math.Max(0, Math.Min(depthX * 320, 320));  //convert to 320, 240 space
            depthY = Math.Max(0, Math.Min(depthY * 240, 240));  //convert to 320, 240 space
            int colorX, colorY;
            var iv = new ImageViewArea();
            // only ImageResolution.Resolution640x480 is supported at this point
            kinect.NuiCamera.GetColorPixelCoordinatesFromDepthPixel(ImageResolution.Resolution640x480, iv, (int)depthX, (int)depthY, (short)0, out colorX, out colorY);
            //colorX = colorX / 2;
            //colorY = colorY / 2;

            // map back to skeleton.Width & skeleton.Height
            var newX = (int)(CameraViewer.ActualWidth * colorX / 640.0) - 30;
            var intY = (int)(CameraViewer.ActualHeight * colorY / 480) - 30;
            return new Point(newX, intY);
        }

        private void MoveImageHats(Point newPosition, int skeletonId)
        {
            if (skeletonId == 1)
            {
                Canvas.SetLeft(this.contentControlHat1, newPosition.X);
                Canvas.SetTop(this.contentControlHat1, newPosition.Y);
            }
            if (skeletonId == 2)
            {
                Canvas.SetLeft(this.contentControlHat2, newPosition.X);
                Canvas.SetTop(this.contentControlHat2, newPosition.Y);
            }
        }

        private void DisplayImageHats(int skeletonId)
        {
            if (skeletonId == 1)
            {
                this.imageHat1.Visibility = Visibility.Visible;
            }
            if (skeletonId == 2)
            {
                this.imageHat2.Visibility = Visibility.Visible;
            }
        }

        private void HideImageHats()
        {
            this.imageHat1.Visibility = Visibility.Hidden;
            this.imageHat2.Visibility = Visibility.Hidden;
        }

        #endregion

        #region Kinect Video Management
        void KinectVideoFrameReady(object sender, ImageFrameReadyEventArgs e)
        {
            var webcameraImage = e.ImageFrame.Image;
            if (this.imageHelper == null)
            {
                this.imageHelper = new InteropBitmapHelper(webcameraImage.Width, webcameraImage.Height, webcameraImage.Bits);
                CameraViewer.Source = this.imageHelper.InteropBitmap;
            }
            else
            {
                this.imageHelper.UpdateBits(webcameraImage.Bits);
            }
        }
        #endregion

        #region Init Kinect Sensor

        void InitKinectSensor()
        {
            this.kinect.Initialize(KinectRuntimeOptions);
            this.kinect.VideoStream.Open(ImageStreamType.Video, 2, ImageResolution.Resolution640x480, ImageType.Color);
            this.kinect.VideoFrameReady += KinectVideoFrameReady;
            this.kinect.SkeletonFrameReady += KinectSkeletonFrameReady;
        }

        public void FindKinectSensor()
        {
            //listen to any status change for Kinects
            Runtime.Kinects.StatusChanged += KinectsStatusChanged;

            //loop through all the Kinects attached to this PC, and start the first that is connected without an error.
            foreach (var runtime in Runtime.Kinects.Where(runtime => runtime.Status == KinectStatus.Connected).Where(runtime => kinect == null))
            {
                this.kinect = runtime;
                InitKinectSensor();
                this.kinectCanvas = new KinectCanvas(kinect, canvasSkeleton);
                return;
            }
        }

        /// <summary>
        /// Handles the StatusChanged event of the Kinects control.
        /// </summary>
        /// <param name="sender">The source of the event.</param>
        /// <param name="e">The <see cref="Microsoft.Research.Kinect.Nui.StatusChangedEventArgs"/> instance containing the event data.</param>
        private void KinectsStatusChanged(object sender, StatusChangedEventArgs e)
        {
            switch (e.Status)
            {
                case KinectStatus.Connected:
                    if (kinect == null)
                    {
                        kinect = e.KinectRuntime;
                        InitKinectSensor();
                        this.kinectCanvas = new KinectCanvas(kinect, canvasSkeleton);
                    }
                    break;
                case KinectStatus.Disconnected:
                    if (kinect == e.KinectRuntime)
                    {
                        kinect = null;
                    }
                    break;
                case KinectStatus.NotReady:
                    if (kinect == null)
                    {
                        //ShowStatus(ErrorCondition.NotReady);
                    }
                    break;
                case KinectStatus.NotPowered:
                    if (kinect == e.KinectRuntime)
                    {
                        kinect = null;
                    }
                    break;
                default:
                    throw new Exception("Unhandled Status: " + e.Status);
            }
            if (kinect == null)
            {
                //  ShowStatus(ErrorCondition.NoKinect);
            }
        }
        #endregion

        #region Paint Skeleton

        private void PaintJoints(SkeletonData skeleton)
        {
            foreach (Joint joint in skeleton.Joints)
            {
                var jointPos = kinectCanvas.GetDisplayPosition(joint);
                var jointLine = new Line
                {
                    X1 = jointPos.X - 3
                };
                jointLine.X2 = jointLine.X1 + 6;
                jointLine.Y1 = jointLine.Y2 = jointPos.Y;
                jointLine.Stroke = KinectCanvas.JointColors[joint.ID];
                jointLine.StrokeThickness = 6;
                canvasSkeleton.Children.Add(jointLine);
            }
        }

        private void PaintBones(SkeletonData skeleton)
        {
            var brush = new SolidColorBrush(Colors.Yellow);
            canvasSkeleton.Children.Add(kinectCanvas.GetBodySegment
                (skeleton.Joints, brush, JointID.HipCenter,
                JointID.Spine, JointID.ShoulderCenter, JointID.Head));
            canvasSkeleton.Children.Add(kinectCanvas.GetBodySegment
                (skeleton.Joints, brush, JointID.ShoulderCenter,
                JointID.ShoulderLeft, JointID.ElbowLeft, JointID.WristLeft, JointID.HandLeft));
            canvasSkeleton.Children.Add(kinectCanvas.GetBodySegment
                (skeleton.Joints, brush, JointID.ShoulderCenter,
                JointID.ShoulderRight, JointID.ElbowRight, JointID.WristRight, JointID.HandRight));
            canvasSkeleton.Children.Add(kinectCanvas.GetBodySegment
                (skeleton.Joints, brush, JointID.HipCenter, JointID.HipLeft,
                JointID.KneeLeft, JointID.AnkleLeft, JointID.FootLeft));
            canvasSkeleton.Children.Add(kinectCanvas.GetBodySegment
                (skeleton.Joints, brush, JointID.HipCenter, JointID.HipRight,
                JointID.KneeRight, JointID.AnkleRight, JointID.FootRight));
        }

        #endregion
    }
}
